import math
import os
import shutil

import numpy as np
from scipy.optimize import linear_sum_assignment


############################### 一些基本的距离、角度计算函数 #############################
def Go_to_xy(init_x, init_y, goal_x, goal_y, vel_limit=[3, 15]):
    """
    输入初始位置和目标位置，返回到极坐标系下的速度和角度
    """
    dx = goal_x - init_x
    dy = goal_y - init_y
    vel = np.clip(np.sqrt(dx**2 + dy**2), vel_limit[0], vel_limit[1])
    angle = np.degrees(np.arctan2(dy, dx))
    # 检查是否为 NaN
    if math.isnan(angle):
        angle = 0
    return vel, 90 - angle


def track_path(pos, heading, path, dt, controller):
    angular_velocity, desired_heading = controller.track_path(pos, heading, path, dt)
    return angular_velocity * dt


def Go_to_va(ref_data, Refe_data, indices):
    """
    根据参考点和极坐标系下的角度和距离，返回目标位置
    """
    ref_x, ref_y = ref_data
    distance, angle_degrees, _, _ = Refe_data[indices[0]]
    # 将角度从度数转换为弧度
    angle_radians = np.radians(90 - angle_degrees)
    # 计算目标位置
    dx = ref_x + distance * np.cos(angle_radians)
    dy = ref_y + distance * np.sin(angle_radians)
    return dx, dy


def caculate_ref_pos(Refe_data):
    """
    返回参考点的位置，索引0处为参考点
    """
    ref_pos = []  # ([it.relatDis, it.relatYaw, it.USVID, it.type])

    for info in Refe_data:
        distance, angle_degrees, usvid, pos_type = info
        angle_radians = np.radians(90 - angle_degrees)
        ref_pos.append(
            [
                distance * np.cos(angle_radians),
                distance * np.sin(angle_radians),
                usvid,
                pos_type,
                distance,
            ]
        )
    ref_pos.sort(key=lambda x: x[4])
    return ref_pos


def Go_to_xy2(init_x, init_y, goal_x, goal_y, max_distance=1):
    """
    输入初始位置和目标位置，返回目标航点
    """
    # 计算目标点与当前点的距离
    distance = np.sqrt((goal_x - init_x) ** 2 + (goal_y - init_y) ** 2)
    # 如果距离小于最大距离，直接返回目标点
    if distance <= max_distance:
        dx = 0
        dy = 0
    # 否则，按比例缩放到最大距离
    else:
        ratio = max_distance / distance
        dx = (goal_x - init_x) * ratio
        dy = (goal_y - init_y) * ratio
    new_x = init_x + dx
    new_y = init_y + dy
    return [new_x, new_y]


######################################### 文件处理函数 ##################################
# 替换Config文件夹
def copy_files(source_dir, destination_dir):
    # 确保目标文件夹存在
    os.makedirs(destination_dir, exist_ok=True)

    # 复制文件夹中的所有内容
    for item in os.listdir(source_dir):
        source_path = os.path.join(source_dir, item)
        destination_path = os.path.join(destination_dir, item)

        if os.path.isdir(source_path):
            # 如果是文件夹，递归复制
            shutil.copytree(source_path, destination_path, dirs_exist_ok=True)
        else:
            # 如果是文件，直接复制
            shutil.copy2(source_path, destination_path)


# 清空dataRecord文件夹
def clear_dataRecord_folder():
    folder_path = "dataRecord"  # 指定 dataRecord 文件夹的路径
    for filename in os.listdir(folder_path):
        file_path = os.path.join(folder_path, filename)
        try:
            if os.path.isfile(file_path) or os.path.islink(file_path):
                os.unlink(file_path)
            elif os.path.isdir(file_path):
                shutil.rmtree(file_path)
        except Exception as e:
            print(f"Failed to delete {file_path}. Reason: {e}")


class suppress_stdout_stderr(object):
    """
    A context manager for doing a "deep suppression" of stdout and stderr in
    Python, i.e. will suppress all print, even if the print originates in a
    compiled C/Fortran sub-function.
       This will not suppress raised exceptions, since exceptions are printed
    to stderr just before a script exits, and after the context manager has
    exited (at least, I think that is why it lets exceptions through).

    """

    def __init__(self):
        # Open a pair of null files
        self.null_fds = [os.open(os.devnull, os.O_RDWR) for x in range(2)]
        # Save the actual stdout (1) and stderr (2) file descriptors.
        self.save_fds = (os.dup(1), os.dup(2))

    def __enter__(self):
        # Assign the null pointers to stdout and stderr.
        os.dup2(self.null_fds[0], 1)
        os.dup2(self.null_fds[1], 2)

    def __exit__(self, *_):
        # Re-assign the real stdout/stderr back to (1) and (2)
        os.dup2(self.save_fds[0], 1)
        os.dup2(self.save_fds[1], 2)
        # Close the null files
        os.close(self.null_fds[0])
        os.close(self.null_fds[1])


########################################### 仿真判断算法 #######################################
def is_done(multiUSVStatus_list, target_location, threshold=10):
    """
    判断是否到达目标位置
    """
    refer_location = target_location[0]
    for i, usv in enumerate(multiUSVStatus_list):
        distance = np.sqrt(
            (refer_location[i][0] - usv[0]) ** 2 + (refer_location[i][1] - usv[1]) ** 2
        )
        print(f"USV {i} 距离目标位置的距离为：{distance}")
        if distance > threshold:
            return False
        if usv[0] < 1000:
            return False
    return True


########################################### 算法 #######################################


def Algorithm_cbs(
    i,
    multiUSVStatus_list,
    refer_location,
    Refe_data,
    indices,
    path,
    controller,
    dt,
):
    """
    CBS
    """
    #

    pos_x, pos_y, yaw, USV_Id = (
        multiUSVStatus_list[i].dbCurX,
        multiUSVStatus_list[i].dbCurY,
        multiUSVStatus_list[i].fCurYaw,
        multiUSVStatus_list[i].iUSVId,
    )
    refer_1 = Go_to_va(refer_location[0], Refe_data, indices)  # 最重要形成的参考点位置
    heading = track_path(
        [pos_x + 200, pos_y + 400], np.deg2rad(90 - yaw), path, dt, controller
    )
    yaw -= np.rad2deg(heading)
    refer_2 = [pos_x + np.cos(yaw) * dt, pos_y + np.sin(yaw) * dt]

    return (refer_1, refer_2, yaw)


def adjust_ass(multiUSVStatus_list, target, low_speed=4, find_speed=10, full_speed=14):
    for it in multiUSVStatus_list:
        if not -10 < it.fCurYaw - 90 < 10:
            return [], []
    vel = []
    yaw = []
    is_location = [False] * len(multiUSVStatus_list)
    for i, it in enumerate(multiUSVStatus_list):
        x_pos = it.dbCurX
        y_pos = it.dbCurY
        yaw_ = it.fCurYaw
        if x_pos > target[i][0] + 3:
            if find_speed - (x_pos - target[i][0]) / 2 < low_speed:
                vel.append(low_speed)
            else:
                vel.append(find_speed - (x_pos - target[i][0]) / 2)
        elif x_pos < target[i][0] - 3:
            if find_speed - (x_pos - target[i][0]) / 2 > full_speed:
                vel.append(full_speed)
            else:
                vel.append(find_speed - (x_pos - target[i][0]) / 2)
        else:
            vel.append(find_speed)
            is_location[i] = True
        if -10 > y_pos - target[i][1] or 10 < y_pos - target[i][1]:
            yaw.append(yaw_)
        if y_pos > target[i][1] + 2:
            yaw.append(180)
        elif y_pos < target[i][1] - 2:
            yaw.append(0)
        else:
            yaw.append(90)
        if all(is_location):
            vel = [full_speed] * len(multiUSVStatus_list)
    return vel, yaw
